#ifndef PID_H
#define PID_H

#include "adc.h"
#include "motor.h"
#include "configuration.h"

//PID OP
float lastError;

void InitPID()
{
    lastError = 0;
}

float getError(float leftGain, float rightGain)
{
    float leftReading  = getAnalog(LEFT_SENSOR)*leftGain;
    float rightReading = getAnalog(RIGHT_SENSOR)*rightGain;

    float error = leftReading - rightReading;

    if (leftReading < READING_THRESH && rightReading < READING_THRESH) // totally off
        error = (lastError > 0) ? 1 : -1;
    else
        error /= (leftReading + rightReading);
   
    if (abs(error) <= ERROR_THRESH)
        error *= 0.00001; // preserve sign just in case

    return error * 100; //about 0 - 70, 100 if both OPs have readings too low.
}

float getPIDAdjustment(int diff, float p, float d)
{
    float currError = getError(1, 1);

    float pAdj = p * currError;
    //int16 iAdj = i * totalError;
    float dAdj = ((float)d * (float)(currError - lastError) / (float)(diff));

    lastError = currError;
    return (pAdj + dAdj);
}

void pidUpdate(int diff, float p, float d, int vel)
{
    int speedAdj = getPIDAdjustment(diff, p, d);

    int leftSpeed = vel - speedAdj;
    int rightSpeed = vel + speedAdj;

    setMotorSpeed(MOTOR_LEFT,  leftSpeed);
    setMotorSpeed(MOTOR_RIGHT, rightSpeed);
}

#endif